#include "scope.h"

static int g_scope_snap_clk_offset;
static int g_scope_snap_data_offset;
static bool g_scope_hw_psram_finish;
static uint32_t g_scope_hw_psram_pos;

void scope_hw_set_sample_rate(uint32_t rate)
{
    uint32_t freq; 
    uint32_t div;

    freq = clock_get_hz(clk_sys);
    div = freq / 2 / g_scope_cfg.rate;
    if (div > 65535) {
        div = 65535;
    }
    pio_sm_set_clkdiv(SCOPE_ADC_PIO, SCOPE_ADC_CLK_SM, div);
}

static void scope_hw_set_ch0_coupling(SCOPE_COUPLING coupling)
{
    if (coupling == SCOPE_COUPLING_DC) {
        pca9557_gpio_set(PCA_CH0_S0, 1);
    } else {
        pca9557_gpio_set(PCA_CH0_S0, 0);
    }
}

static void scope_hw_set_ch1_coupling(SCOPE_COUPLING coupling)
{
    if (coupling == SCOPE_COUPLING_DC) {
        pca9557_gpio_set(PCA_CH1_S0, 1);
    } else {
        pca9557_gpio_set(PCA_CH1_S0, 0);
    }
}

void scope_hw_set_ch_coupling(int ch, SCOPE_COUPLING coupling)
{
    if (ch == 0) {
        scope_hw_set_ch0_coupling(coupling);
    } else {
        scope_hw_set_ch1_coupling(coupling);
    }
}

static void scope_hw_set_ch0_scale(int scale)
{
    if (scale == 0) {
        pca9557_gpio_set(PCA_CH0_S1, 0);
        pca9557_gpio_set(PCA_CH0_S2, 1);
    }
    if (scale == 1) {
        pca9557_gpio_set(PCA_CH0_S1, 0);
        pca9557_gpio_set(PCA_CH0_S2, 0);
    }
    if (scale == 2) {
        pca9557_gpio_set(PCA_CH0_S1, 1);
        pca9557_gpio_set(PCA_CH0_S2, 0);
    }
}

static void scope_hw_set_ch1_scale(int scale)
{
    if (scale == 0) {
        pca9557_gpio_set(PCA_CH1_S1, 0);
        pca9557_gpio_set(PCA_CH1_S2, 1);
    }
    if (scale == 1) {
        pca9557_gpio_set(PCA_CH1_S1, 0);
        pca9557_gpio_set(PCA_CH1_S2, 0);
    }
    if (scale == 2) {
        pca9557_gpio_set(PCA_CH1_S1, 1);
        pca9557_gpio_set(PCA_CH1_S2, 0);
    }
}

void scope_hw_set_ch_scale(int ch, int scale)
{
    if (ch == 0) {
        scope_hw_set_ch0_scale(scale);
    } else {
        scope_hw_set_ch1_scale(scale);
    }
}

void scope_hw_update_ch_scale(void)
{ 
    scope_hw_set_ch0_scale(g_scope_cfg.ch0_scale);
    scope_hw_set_ch1_scale(g_scope_cfg.ch1_scale);
}

void scope_hw_auto_ch_scale(void)
{ 
    if (g_scope_cfg.running) {
        if (g_scope_cfg.ch0_div_mv > 1280) {
            g_scope_cfg.ch0_scale = 2;
        } else if (g_scope_cfg.ch0_div_mv > 1280 / 5) {
            g_scope_cfg.ch0_scale = 1;
        } else {
            g_scope_cfg.ch0_scale = 0;
        }

        if (g_scope_cfg.ch1_div_mv > 1280) {
            g_scope_cfg.ch1_scale = 2;
        } else if (g_scope_cfg.ch1_div_mv > 1280 / 5) {
            g_scope_cfg.ch1_scale = 1;
        } else {
            g_scope_cfg.ch1_scale = 0;
        }      
        scope_hw_update_ch_scale();
    }
}

void scope_hw_poll(void)
{
    bool running = SCOPE_ADC_PIO->ctrl & (1u << SCOPE_ADC_CLK_SM);
    if (g_scope_cfg.running == true && running == false) {
        g_scope_cfg.running = false;
        scope_hw_stop();
        scope_snap_psram_stop();
    }
}

static void scope_hw_init_alloc(void)
{
    pio_sm_claim(SCOPE_ADC_PIO, SCOPE_ADC_CLK_SM);
    pio_sm_claim(SCOPE_ADC_PIO, SCOPE_ADC_TO_BUFFER_SM);
    pio_sm_claim(SCOPE_ADC_PIO, SCOPE_ADC_TO_PSRAM_SM);

    g_scope_cfg.dma_chan_adc_data = dma_claim_unused_channel(true);
    g_scope_cfg.dma_chan_psram_data = dma_claim_unused_channel(true);
    g_scope_cfg.dma_chan_psram_ctrl = dma_claim_unused_channel(true);

    g_scope_snap_data_offset = pio_add_program(SCOPE_ADC_PIO, &sample_program);
    g_scope_snap_clk_offset = pio_add_program(SCOPE_ADC_PIO, &clk_program);
}

static void scope_hw_init_pio_sample_data(void)
{
    pio_sm_config c = sample_program_get_default_config(g_scope_snap_data_offset);
    sm_config_set_in_pin_base(&c, PIN_ADC_D0);
    sm_config_set_clkdiv(&c, 1);
    sm_config_set_in_shift(&c, true, true, 32);
    // sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_RX);
    pio_sm_init(SCOPE_ADC_PIO, SCOPE_ADC_TO_BUFFER_SM, g_scope_snap_data_offset, &c);
    pio_sm_init(SCOPE_ADC_PIO, SCOPE_ADC_TO_PSRAM_SM, g_scope_snap_data_offset, &c);
}

static void scope_hw_init_pio_sample_clk(void)
{ 
    int freq = clock_get_hz(clk_sys);

    pio_gpio_init(SCOPE_ADC_PIO, PIN_ADC_CLK);
    pio_sm_set_consecutive_pindirs(SCOPE_ADC_PIO, SCOPE_ADC_CLK_SM, PIN_ADC_CLK, 1, true);
    pio_sm_config c = clk_program_get_default_config(g_scope_snap_clk_offset);
    sm_config_set_sideset_pins(&c, PIN_ADC_CLK);
    sm_config_set_clkdiv(&c, freq / 2 / g_scope_cfg.rate);
    pio_sm_init(SCOPE_ADC_PIO, SCOPE_ADC_CLK_SM, g_scope_snap_clk_offset, &c);
}

void scope_hw_init_dma_adc_to_buffer(void)
{
    dma_channel_config cfg;

    dma_channel_abort(g_scope_cfg.dma_chan_adc_data);
    cfg = dma_channel_get_default_config(g_scope_cfg.dma_chan_adc_data);
    channel_config_set_transfer_data_size(&cfg, DMA_SIZE_32);
    channel_config_set_read_increment(&cfg, false);
    channel_config_set_write_increment(&cfg, true);
    channel_config_set_dreq(&cfg, SCOPE_ADC_BUFFER_RX_REQ);
    // channel_config_set_high_priority(&cfg, true);

    dma_channel_configure(
        g_scope_cfg.dma_chan_adc_data,
        &cfg,
        scope_snap_buffer_get_dma_target(),
        &SCOPE_ADC_PIO->rxf[SCOPE_ADC_TO_BUFFER_SM], 
        SCOPE_ADC_BUFFER_WORDS,
        true
        );
}

static void scope_hw_init_dma_adc_to_psram_data(void)
{
    dma_channel_config cfg;

    dma_channel_abort(g_scope_cfg.dma_chan_psram_data);
    cfg = dma_channel_get_default_config(g_scope_cfg.dma_chan_psram_data);
    channel_config_set_transfer_data_size(&cfg, DMA_SIZE_32);
    channel_config_set_read_increment(&cfg, false);
    channel_config_set_write_increment(&cfg, false);
    channel_config_set_chain_to(&cfg, g_scope_cfg.dma_chan_psram_ctrl);
    channel_config_set_dreq(&cfg, SCOPE_ADC_PSRAM_RX_REQ);
    channel_config_set_high_priority(&cfg, true);

    dma_channel_configure(
        g_scope_cfg.dma_chan_psram_data,
        &cfg,
        psram_save_start(),
        &SCOPE_ADC_PIO->rxf[SCOPE_ADC_TO_PSRAM_SM], 
        psram_size() / sizeof(uint32_t),
        false
        );
}

static void scope_hw_init_dma_adc_to_psram_ctrl(void)
{
    static uint32_t psram_restart_count;
    dma_channel_config cfg;

    psram_restart_count = psram_size() / sizeof(uint32_t);
    dma_channel_abort(g_scope_cfg.dma_chan_psram_ctrl);
    cfg = dma_channel_get_default_config(g_scope_cfg.dma_chan_psram_ctrl);
    channel_config_set_transfer_data_size(&cfg, DMA_SIZE_32);
    channel_config_set_read_increment(&cfg, false);
    channel_config_set_write_increment(&cfg, false);
    channel_config_set_dreq(&cfg, DREQ_FORCE);
    channel_config_set_high_priority(&cfg, true);

    dma_channel_configure(
        g_scope_cfg.dma_chan_psram_ctrl,
        &cfg,
        &dma_channel_hw_addr(g_scope_cfg.dma_chan_psram_data)->al1_transfer_count_trig,
        &psram_restart_count, 
        1,
        true
        );
}

static void scope_hw_snap_init_pio(void)
{ 
    scope_hw_init_pio_sample_data();
    scope_hw_init_pio_sample_clk();
}

static void scope_snap_stop_sm(void)
{ 
    pio_sm_set_enabled(SCOPE_ADC_PIO, SCOPE_ADC_CLK_SM, false);
    log_printf("scope_snap_stop_sm\n");
    pio_sm_set_enabled(SCOPE_ADC_PIO, SCOPE_ADC_TO_BUFFER_SM, false);
    pio_sm_set_enabled(SCOPE_ADC_PIO, SCOPE_ADC_TO_PSRAM_SM, false);
}

static void scope_snap_start_sm(void)
{ 
    pio_sm_set_enabled(SCOPE_ADC_PIO, SCOPE_ADC_TO_BUFFER_SM, true);
    pio_sm_set_enabled(SCOPE_ADC_PIO, SCOPE_ADC_TO_PSRAM_SM, true);
    pio_sm_set_enabled(SCOPE_ADC_PIO, SCOPE_ADC_CLK_SM, true);
}

static void scope_snap_stop_dma(void)
{ 
    log_printf("scope_snap_stop_dma\n");
    g_scope_hw_psram_finish = dma_hw->intr & (1u << g_scope_cfg.dma_chan_psram_data);
    g_scope_hw_psram_pos = psram_size() - dma_hw->ch[g_scope_cfg.dma_chan_psram_data].transfer_count * sizeof(uint32_t);
    log_printf("finish:%d pos:%d\n", g_scope_hw_psram_finish, g_scope_hw_psram_pos);

    dma_channel_acknowledge_irq0(g_scope_cfg.dma_chan_psram_data);
    dma_channel_cleanup(g_scope_cfg.dma_chan_adc_data);
    dma_channel_cleanup(g_scope_cfg.dma_chan_psram_ctrl);
    dma_channel_cleanup(g_scope_cfg.dma_chan_psram_data);
}

bool scope_hw_psram_finish(void)
{
    return g_scope_hw_psram_finish;
}

uint32_t scope_hw_psram_pos(void)
{
    return g_scope_hw_psram_pos;
}

void scope_hw_stop(void)
{
    scope_snap_stop_sm();
    scope_snap_stop_dma();
    psram_save_stop();
}

void scope_hw_run(void)
{
    scope_hw_stop();
    scope_hw_snap_init_pio();
    scope_hw_init_dma_adc_to_buffer();
    scope_hw_init_dma_adc_to_psram_data();
    scope_hw_init_dma_adc_to_psram_ctrl();
    scope_snap_start_sm();
}

void scope_hw_init(void)
{
    scope_hw_set_ch0_coupling(SCOPE_COUPLING_DC);
    scope_hw_set_ch1_coupling(SCOPE_COUPLING_DC);
    g_scope_cfg.ch0_scale = 1;
    g_scope_cfg.ch1_scale = 1;
    scope_hw_update_ch_scale();    
    scope_hw_init_alloc();
}
